#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include "ohos_init.h"
#include "cmsis_os2.h"

#include "hi_wifi_api.h"
#include "lwip/ip_addr.h"
#include "lwip/netifapi.h"
#include "lwip/sockets.h"

#include "MQTTClient.h"
#include "robot_control.h"
#include "oled_display.h"

static MQTTClient mq_client;

unsigned char *onenet_mqtt_buf;
unsigned char *onenet_mqtt_readbuf;
int buf_size;
unsigned char g_actual_speed = 0;  
unsigned char g_speed = 0;
Network n;
MQTTPacket_connectData data = MQTTPacket_connectData_initializer;

// 外部变量声明
extern unsigned char g_car_status;
extern unsigned char g_current_level_index;
extern unsigned char g_left_motor_enable;
extern unsigned char g_right_motor_enable;
extern unsigned char g_left_motor_direction;
extern unsigned char g_right_motor_direction;
extern void car_forward(void);
extern void car_backward(void);
extern void car_left(void);
extern void car_right(void);
extern void car_stop(void);
extern void set_motor_speed_level(unsigned char speed_level);
extern void oled_display_car_status(unsigned char car_status, unsigned char speed_level);

// 速度等级映射表 - 对应robot_control.h中的定义
static const unsigned char speed_level_map[6] = {
    30,   // Level 0: Stop
    50,  // Level 1: Low speed
    70,  // Level 2: Medium-low speed
    90,  // Level 3: Medium-high speed
    90,  // Level 4: High speed
    100  // Level 5: Maximum speed
};

// 获取当前行车状态字符串
const char* get_car_status_string(void)
{
    switch (g_car_status) {
        case 0: return "STOP";                    // CAR_STOP_STATUS
        case 1: return "OBSTACLE_AVOIDANCE";     // CAR_OBSTACLE_AVOIDANCE_STATUS
        case 2: return "TRACE";                  // CAR_TRACE_STATUS
        case 3: return "REMOTE_CONTROL";         // CAR_REMOTE_CONTROL_STATUS
        default: return "UNKNOWN";
    }
}

// 获取当前动作字符串
const char* get_current_action_string(void)
{
    if (!g_left_motor_enable && !g_right_motor_enable) {
        return "STOP";
    } else if (g_left_motor_enable && g_right_motor_enable) {
        if (g_left_motor_direction == 0 && g_right_motor_direction == 0) {
            return "FORWARD";
        } else if (g_left_motor_direction == 1 && g_right_motor_direction == 1) {
            return "BACKWARD";
        } else {
            return "TURN";
        }
    } else if (g_left_motor_enable && !g_right_motor_enable) {
        return "RIGHT";
    } else if (!g_left_motor_enable && g_right_motor_enable) {
        return "LEFT";
    } else {
        return "UNKNOWN";
    }
}

// 简化的命令队列 - 避免栈溢出
#define MAX_CMD_QUEUE_SIZE 4
#define MAX_CMD_LENGTH 128  // 增加缓冲区大小以容纳完整的JSON消息

typedef struct {
    char command[MAX_CMD_LENGTH];
    int valid;
} command_queue_item_t;

static command_queue_item_t cmd_queue[MAX_CMD_QUEUE_SIZE];
static int queue_head = 0;
static int queue_tail = 0;
static int queue_count = 0;

// 添加命令到队列 - 简化版本
static int add_command_to_queue(const char* cmd) {
    if (queue_count >= MAX_CMD_QUEUE_SIZE) {
        return -1;
    }
    
    // 清理并复制命令
    memset(cmd_queue[queue_tail].command, 0, MAX_CMD_LENGTH);
    int len = strlen(cmd);
    if (len >= MAX_CMD_LENGTH) {
        len = MAX_CMD_LENGTH - 1;
    }
    memcpy(cmd_queue[queue_tail].command, cmd, len);
    cmd_queue[queue_tail].valid = 1;
    
    queue_tail = (queue_tail + 1) % MAX_CMD_QUEUE_SIZE;
    queue_count++;
    return 0;
}

// 从队列获取命令 - 简化版本
static int get_command_from_queue(char* cmd, int max_len) {
    if (queue_count <= 0 || !cmd_queue[queue_head].valid) {
        return -1;
    }
    
    memset(cmd, 0, max_len);
    int len = strlen(cmd_queue[queue_head].command);
    if (len >= max_len) {
        len = max_len - 1;
    }
    memcpy(cmd, cmd_queue[queue_head].command, len);
    
    // 清理队列项
    memset(&cmd_queue[queue_head], 0, sizeof(command_queue_item_t));
    queue_head = (queue_head + 1) % MAX_CMD_QUEUE_SIZE;
    queue_count--;
    return 0;
}

// 简化的控制命令处理 - 避免栈溢出
static void process_control_command_simple(const char* message)
{
    // 添加调试信息，显示接收到的完整消息
    printf("RECV_MSG:%s\n", message);

    // 检查远程控制模式
    if (g_car_status != 3) { // 3 = CAR_REMOTE_CONTROL_STATUS
        printf("NOT_REMOTE\n");
        return;
    }
    
    // 简化的命令解析 - 移除OLED调用，避免与ssd1306_test.c冲突
    if (strstr(message, "forward")) {
        printf("FWD\n");
        car_forward();
    }
    else if (strstr(message, "backward")) {
        printf("BWD\n");
        car_backward();
    }
    else if (strstr(message, "left")) {
        printf("LEFT\n");
        car_left();
    }
    else if (strstr(message, "right")) {
        printf("RIGHT\n");
        car_right();
    }
    else if (strstr(message, "stop")) {
        printf("STOP\n");
        car_stop();
    }
    else if (strstr(message, "\"action\":\"set_speed\"")) 
    {
        // 解析速度等级 - 参考您提供的正确代码格式
        char* speed_pos = strstr(message, "\"speed_level\":");
        if (speed_pos) 
        {
            int speed = 0;
            sscanf(speed_pos, "\"speed_level\":%d", &speed);
            if (speed >= 0 && speed <= 5) {
                printf("✅ Executing: SET_SPEED to level %d\n", speed);

                // 使用速度等级映射表转换为实际速度值
                unsigned char actual_speed = speed_level_map[speed];
                set_motor_speed_level(actual_speed);
                g_actual_speed = actual_speed;  // 更新实际速度变量
                g_speed = speed;
                // 更新全局速度等级变量
                g_current_level_index = speed;


                printf("Speed mapped: Level %d -> Actual Speed %d\n", speed, actual_speed);
            } else {
                printf("❌ Invalid speed level: %d\n", speed);
            }
        } 
        else 
        {
            printf("❌ No speed_level field found\n");
        }
    }
}

//消息回调函数 - 简化版本避免栈溢出
void mqtt_callback(MessageData *msg_data)
{
    if (msg_data == NULL) {
        printf("CB_NULL\n");
        return;
    }

    // 简化打印，避免长字符串
    printf("MQTT_MSG\n");

    // 检查是否是控制命令主题
    if (msg_data->topicName->lenstring.len == 11 &&
        strncmp(msg_data->topicName->lenstring.data, "car/control", 11) == 0) {

        // 限制消息长度，避免栈溢出
        int len = msg_data->message->payloadlen;
        printf("MSG_LEN:%d\n", len);  // 调试：显示消息长度

        if (len > MAX_CMD_LENGTH - 1) {
            printf("MSG_TRUNCATED:%d->%d\n", len, MAX_CMD_LENGTH - 1);
            len = MAX_CMD_LENGTH - 1;
        }

        // 直接添加到队列，避免在回调中处理
        char temp_cmd[MAX_CMD_LENGTH];
        memset(temp_cmd, 0, MAX_CMD_LENGTH);
        memcpy(temp_cmd, msg_data->message->payload, len);
        temp_cmd[len] = '\0';
        
        if (add_command_to_queue(temp_cmd) == 0) {
            printf("CMD_QUEUED\n");
        } else {
            printf("QUEUE_FULL\n");
        }
    }
}

int mqtt_connect(void)
{
    int rc = 0;
    extern int wifi_ok_flg;
    int retry_count = 0;
    const int max_retries = 10;

    printf("MQTT_START\n");

    // 等待WiFi连接
    while (!wifi_ok_flg && retry_count < max_retries) {
        printf("WAIT_WIFI %d\n", retry_count + 1);
        usleep(1000000);
        retry_count++;
    }

    if (!wifi_ok_flg) {
        printf("WIFI_TIMEOUT\n");
        return -1;
    }

    printf("WIFI_OK\n");

    // 网络连接 - 使用原始成功方式
    NetworkInit(&n);
    rc = NetworkConnect(&n, "192.168.22.157", 1883);
    if (rc != 0) {
        printf("NET_FAIL %d\n", rc);
        return -2;
    }
    printf("NET_OK\n");

    // 缓冲区分配 - 使用原始成功方式
    buf_size = 4096 + 1024;
    onenet_mqtt_buf = (unsigned char *) malloc(buf_size);
    onenet_mqtt_readbuf = (unsigned char *) malloc(buf_size);
    if (!(onenet_mqtt_buf && onenet_mqtt_readbuf)) {
        printf("MEM_FAIL\n");
        return -3;
    }
    printf("MEM_OK\n");

    // MQTT客户端初始化 - 使用原始成功方式
    MQTTClientInit(&mq_client, &n, 1000, onenet_mqtt_buf, buf_size, onenet_mqtt_readbuf, buf_size);
    MQTTStartTask(&mq_client);
    printf("CLIENT_OK\n");

    // 连接参数 - 使用原始成功方式
    data.keepAliveInterval = 30;
    data.cleansession = 1;
    data.clientID.cstring = "ohos_hi3861";
    data.username.cstring = "yhb";
    data.password.cstring = "yhb134679";
    data.cleansession = 1;
    
    mq_client.defaultMessageHandler = mqtt_callback;
    printf("HANDLER_SET\n");

    // 连接MQTT服务器
    rc = MQTTConnect(&mq_client, &data);
    if (rc != 0) {
        printf("MQTT_FAIL %d\n", rc);
        return -4;
    }
    printf("MQTT_OK\n");

    // 订阅控制命令主题
    rc = MQTTSubscribe(&mq_client, "car/control", 0, mqtt_callback);
    if (rc != 0) {
        printf("SUB_FAIL %d\n", rc);
        return -5;
    }
    printf("SUB_OK\n");

    printf("LOOP_START\n");

    while(1)
    {
        // 移除OLED显示调用，避免与ssd1306_test.c中的显示任务冲突
        // OLED显示统一由ssd1306_test.c中的Ssd1306TestTask处理

        // 处理队列中的命令
        char cmd_buffer[MAX_CMD_LENGTH];
        static int idle_counter = 0;

        if (get_command_from_queue(cmd_buffer, MAX_CMD_LENGTH) == 0) {
            printf("PROC_CMD\n");
            process_control_command_simple(cmd_buffer);
            idle_counter = 0; // 重置空闲计数器
        } else {
            // 队列为空时的处理
            idle_counter++;

            // 如果队列空闲超过30次循环(3秒)且在远程控制模式下，自动停止
            if (idle_counter >= 30 && g_car_status == 3) {
                if (g_left_motor_enable || g_right_motor_enable) {
                    printf("AUTO_STOP\n");
                    car_stop();
                    // 移除OLED调用，避免与ssd1306_test.c冲突
                }
            }
        }

        // MQTT状态发送和简化的状态输出
        static int status_counter = 0;
        status_counter++;

        if (status_counter >= 10) { // 每10次循环(1秒)发送一次状态
            status_counter = 0;

            // 发送MQTT状态消息 - 使用您提供的正确格式
            MQTTMessage message;
            char status_json[512];

            // 创建详细的状态JSON消息 - 与HTML接口匹配，参考您的正确代码
            snprintf(status_json, sizeof(status_json),
                "{"
                "\"device_id\":\"hi3861_car\","
                "\"status\":\"%s\","
                "\"action\":\"%s\","
                "\"speed_level\":%d,"
                "\"left_motor\":{"
                    "\"enable\":%d,"
                    "\"direction\":%d"
                "},"
                "\"right_motor\":{"
                    "\"enable\":%d,"
                    "\"direction\":%d"
                "},"
                "\"timestamp\":\"%ld\""
                "}",
                get_car_status_string(),
                get_current_action_string(),
                g_current_level_index,
                g_left_motor_enable,
                g_left_motor_direction,
                g_right_motor_enable,
                g_right_motor_direction,
                (long)osKernelGetTickCount()
            );

            message.qos = QOS1;
            message.retained = 0;
            message.payload = (void *)status_json;
            message.payloadlen = strlen(status_json);

            // 发送状态消息到car/status主题
            printf("📤 Publishing status: %s | %s | Speed:%d\n",
                   get_car_status_string(), get_current_action_string(), g_current_level_index);

            if (MQTTPublish(&mq_client, "car/status", &message) < 0) {
                printf("❌ MQTTPublish failed!\n");
            } else {
                printf("✅ Status published successfully\n");
            }
        }

        // 减少循环频率，避免CPU占用过高
        usleep(100000); // 100ms
    }

    return 0;
}

void mqtt_test(void)
{
    printf("MQTT_TEST_START\n");
    mqtt_connect();
}
